#include "Soldier.h"
#include "SteeringBehaviours/SteeringBehaviourOutput.h"
#include "Tools/Tools.h"

#include "Formation/VFormation.h"
#include "Formation/FormationState.h"

#include <cmath>
#include <iostream>

Soldier::Soldier(FormationBody* formationBody, int formationPosition)
{
    STOP_DISTANCE = 3.;
    SLOW_RADIUS = PI/4;
    STOP_RADIUS = PI/10;
    //Création de comportements
    m_seekBehaviour  = new SteeringSeekBehaviour();
    m_alignBehaviour = new SteeringAlignBehaviour(STOP_RADIUS,SLOW_RADIUS);

    m_faceBehaviour = new SteeringFaceBehaviour(STOP_DISTANCE,*m_alignBehaviour);

    //Création du body
    m_body = new SoldierBody(this,0.5,PI/10);

    m_formationBody = formationBody;
    m_formationPosition = formationPosition;
    m_isLeader = (this->m_formationPosition == 0);

}

Soldier::~Soldier(){
    std::cout<<"Destruction Soldier"<<std::endl;
    delete m_seekBehaviour;
    //if (m_formationBody) delete m_formationBody;
}

Soldier::Soldier(Soldier& s){

    //Création de comportements
    m_seekBehaviour  = s.m_seekBehaviour;

    //Création du body
    m_body = s.m_body;

    m_formationBody = s.m_formationBody;
    m_formationPosition = s.m_formationPosition;
    m_isLeader = s.m_isLeader;

}

Soldier& Soldier::operator=(Soldier& s){

    if (!m_seekBehaviour) delete m_seekBehaviour;

    //Création de comportements
    m_seekBehaviour  = s.m_seekBehaviour;

    //Création du body
    m_body = s.m_body;

    if (!m_formationBody) delete m_formationBody;

    m_formationBody = s.m_formationBody;

    m_formationPosition = s.m_formationPosition;
    m_isLeader = s.m_isLeader;

    return *this;
}


//Vie du soldat
void Soldier::live()
{
    Point2D target = Point2D(700,50);

    vector<Perception*> perceptions = getPerceptions();


    if (m_isLeader) {

        // Perceptions
        vector<Perception*> perceptions = getPerceptions();

        if (perceptions.size() != 0) {

           for ( int i = 0; i < perceptions.size(); i++) {
                 if (perceptions[i]->isSoldier()) {
                     cout << "Soldier detected" << endl;
                 } else if (perceptions[i]->isStone()) {
                     cout << "Stone detected" << endl;
                     changeFormationBody(new VFormation());
                     //double newTargetX = target.x() * cos(0.5) - target.y() * sin(0.5);
                     //double newTargetY = target.x() * cos(0.5) + target.y() * sin(0.5);
                     //target += Point2D(newTargetX, newTargetY);
                 } else {
                     cout << "Ghost detected" << endl;
                 }
           }

        }

        //Création du BehaviourOutput
        SteeringBehaviourOutput* output = NULL;

        m_formationBody->setGlobalPosition(m_body->getX(),m_body->getY());
        Vector2D v(cos(m_body->getAngle()),sin(m_body->getAngle()));
        std::cout<<"Orientation :::::::::::"<<m_body->getAngle()<<":::"<<v.x()<<" - "<<v.y()<<std::endl;
        m_formationBody->setGlobalOrientation(v.x(),v.y());
        //m_formationBody->setGlobalOrientation(m_body->getAngle(),m_body->getY());

        output =  m_seekBehaviour->runSeek(Point2D(m_body->getX(),m_body->getY()),10,10,target);
        if (m_seekBehaviour!=NULL){
            SteeringBehaviourOutput* sbo = m_faceBehaviour->runFace(Point2D(m_body->getX(),m_body->getY()),
                                                                    Vector2D(cos(m_body->getAngle()),sin(m_body->getAngle())),
                                                                    10,10,Point2D(400,500));
            std::cout<<"AAAAAAAAAAAAAAAAANNNNNNNNNNNNNNNNNGGGGGGGGGUUUUUUUUULLLLLLLAAAAAAAAARRRR ::::"<<sbo->getAngular()<<std::endl;
            output->setAngular(sbo->getAngular());
        }
        m_body->influence(output->getLinear(),output->getAngular());
        delete output;

    } else {

      SteeringBehaviourOutput* output;
      Point2D target = this->m_formationBody->getSlotAt(this->m_formationPosition).getGlobalPosition();


      // run the seek behaviour
      output = m_seekBehaviour->runSeek(Point2D(m_body->getX(),m_body->getY()), 10, 10, target);

      m_body->influence(output->getLinear(),output->getAngular());

      delete output;

    }

}

bool Soldier::isLeader(){
    return (this->isLeader());
}

std::vector<Perception*> Soldier::getPerceptions(){
    return m_body->getObjectsPerceived();
}

void Soldier::changeFormationBody(FormationBody* formationBody){
    if (m_formationBody->getState() == FormationState(LINEFORMATION))
    {
        formationBody->scale(5);
        (*this->m_formationBody) = *formationBody;
    }
}
